#ifndef ASTAR_PLANNER_H
#define ASTAR_PLANNER_H

#include <iostream>
#include <vector>
#include <queue>
#include <cmath>
#include <unordered_map>
#include <algorithm>
#include <opencv2/opencv.hpp>
#include <yaml-cpp/yaml.h>

// 栅格地图参数（新增膨胀核大小，控制障碍膨胀范围）
struct MapParams
{
    std::string image_path;   // PGM地图文件路径
    double resolution;        // 分辨率（米/像素）
    cv::Point2d origin;       // 地图原点（世界坐标）
    double occupied_thresh;   // 障碍物阈值
    double free_thresh;       // 自由空间阈值
    int negate;               // 是否反转地图（0=不反转，1=反转）
    int dilate_kernel_size;   // 障碍膨胀核大小（如3=3x3核，膨胀1层）
};

// A*节点结构
struct Node
{
    int x, y;                 // 栅格坐标
    double g;                 // 起点到当前节点代价
    double h;                 // 启发式代价（当前到终点）
    double f;                 // 总代价（f = g + h）
    Node *parent;             // 父节点指针

    Node(int x_, int y_, double g_, double h_, Node *parent_);

    // 优先队列排序规则（f值小的优先）
    bool operator>(const Node &other) const;
};

// 解析YAML地图配置文件
MapParams parseYaml(const std::string &yaml_path);

// 加载PGM地图并转换为二值栅格地图（0=自由，1=障碍）
cv::Mat loadMap(const MapParams &params);

// 障碍膨胀（核心函数：扩展障碍物范围）
void dilateObstacles(cv::Mat &grid_map, int kernel_size);

// 计算启发式代价（曼哈顿距离）
double heuristic(int x1, int y1, int x2, int y2);

// A*路径搜索算法
std::vector<cv::Point> aStarSearch(
    const cv::Mat &grid_map,  // 膨胀后的栅格地图
    cv::Point start,          // 栅格起点
    cv::Point goal            // 栅格终点
);

// 世界坐标（米）转栅格坐标
cv::Point worldToGrid(const cv::Point2d &world_pt, const MapParams &params);

// 栅格坐标转世界坐标（米）
cv::Point2d gridToWorld(const cv::Point &grid_pt, const MapParams &params);

#endif // ASTAR_PLANNER_H